#include "project.h"

void projector::initParams()
{        
    std::string config_path = ros::package::getPath("pointcloud2image");
    std::ifstream infile(config_path + "/config/initial_params.txt");
    infile >> i_params.camera_topic;
    // camera topic
    infile >> i_params.lidar_topic;
    // point cloud topic
    
    double_t camtocam[16];
    // external params of camera to camera
    double_t cameraIn[12];
    // inner params of camera
    double_t RT[16];
    // external params of lidar to camera
    
    for (int i = 0; i < 16; i++)
        infile >> camtocam[i];
    cv::Mat(4, 4, 6, &camtocam).copyTo(i_params.camtocam_mat);
    //camera to camera params

    for (int i = 0; i < 12; i++)
        infile >> cameraIn[i];
    cv::Mat(3, 4, 6, &cameraIn).copyTo(i_params.cameraIn);
    //camera inner params

    for (int i = 0; i < 16; i++)
        infile >> RT[i];
    cv::Mat(4, 4, 6, &RT).copyTo(i_params.RT);
    //lidar to camera params
    
    std::cout<<i_params.RT<<std::endl;
}


void projector::projection_callback(const sensor_msgs::Image::ConstPtr &img, 
                                const sensor_msgs::PointCloud2::ConstPtr &pc)
{
    cv_bridge::CvImagePtr cv_ptr;
    try {
        cv_ptr = cv_bridge::toCvCopy(img, "bgr8");
    }
    catch (cv_bridge::Exception &e) {
        return;
    }

    cv::Mat raw_img = cv_ptr->image;
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::fromROSMsg(*pc, *cloud);
    
    cv::Mat visImg = raw_img.clone();
    cv::Mat overlay = visImg.clone();

    cv::Mat X(4,1,cv::DataType<double>::type);
    cv::Mat Y(3,1,cv::DataType<double>::type);
    for(pcl::PointCloud<pcl::PointXYZI>::const_iterator it = cloud->points.begin(); it != cloud->points.end(); it++){
        if(it->x > maxX || it->x < 0.0 || abs(it->y) > maxY || it->z < minZ ) 
            continue;
        
        X.at<double>(0,0) = it->x;
        X.at<double>(1,0) = it->y;
        X.at<double>(2,0) = it->z;
        X.at<double>(3,0) = 1;

        //Y = i_params.cameraIn * i_params.camtocam_mat * i_params.RT * X;  
        //tranform the camera to the camera coordinate
        
        Y = i_params.cameraIn * i_params.RT * X;
        //tranform the point to the camera coordinate
        cv::Point pt;
        pt.x = Y.at<double>(0,0) / Y.at<double>(0,2);
        pt.y = Y.at<double>(1,0) / Y.at<double>(0,2);
        std::cout<<pt.x<<" "<<pt.y<<std::endl;

        float val = it->x;
        float maxVal = 20.0;
        int red = std::min(255, (int)(255 * abs((val - maxVal) / maxVal)));
        int green = std::min(255, (int)(255 * (1 - abs((val - maxVal) / maxVal))));
        cv::circle(overlay, pt, 1, cv::Scalar(0, green, red), -1);
    }


    // Publish the image projection
    ros::Time time = ros::Time::now();
    cv_ptr->encoding = "bgr8";
    cv_ptr->header.stamp = time;
    cv_ptr->header.frame_id = "/camera_main";
    cv_ptr->image = overlay;
    image_publisher.publish(cv_ptr->toImageMsg());
    std::cout<<"project picture is published!"<<std::endl;
}

projector::projector() 
{
    ros::NodeHandle nh("~");
    initParams();

    ros::Publisher project_img_pub;

    message_filters::Subscriber<sensor_msgs::Image> image_sub(nh, i_params.camera_topic, 5);
    message_filters::Subscriber<sensor_msgs::PointCloud2> pcl_sub(nh, i_params.lidar_topic, 5);

    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::PointCloud2> MySyncPolicy;
    message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(5), image_sub, pcl_sub);
    sync.registerCallback(boost::bind(&projector::projection_callback, this, _1, _2));

    image_transport::ImageTransport imageTransport(nh);
    image_publisher = imageTransport.advertise("/projected_image", 20);

    ros::spin();
}

int main(int argc, char **argv){
    ros::init(argc, argv, "project_pc_to_image");
    projector projector;
    return 0;
}
